/** \file main.c
 *
 * Test main program for emulation
 *
 * (c) 11/2004 Martin Strubel <hackfin@section5.ch>
 *
 * $Id: main.c 75 2005-11-11 09:12:34Z jiez $
 *
 *
 * Note: This program was only tested with:
 *
 * - A ADI STAMP board with BF-533 rev 0.3 (0.2)
 * - Xilinx DLC5 compatible parallel port JTAG cable
 *   (see jtag_init() for other cable support)
 * - U-Boot 1.1.1 (Release Version Beta - Nov 20 2004-11:17:00)
 *
 * Usage:
 *
 * Start the STAMP and run this program. It will then wait for an
 * exception caused by accessing memory address 0x200.
 * Type:
 *
 *     md 0 200 
 *
 * on the STAMP serial console. This will cause the expected exceptions
 * for the test program to continue.
 *
 *
 * For other configurations, you will have to tweak the breakpoint address.
 *
 */


#include "emu.h"
#include <unistd.h>
#include <string.h>  // memset()
#include <stdio.h>
#include "jtag.h" // TESTING

#include "construct.h" // size codes for access TODO: move elsewhere

#define __ADSPLPBLACKFIN__  // This define gets rid of warnings
#include "defBF532.h"

const char g_line[] =
	"---------------------------------------------------------------------\n";
const char g_dline[] =
	"=====================================================================\n";

static
unsigned char g_emuexcpt[2] = { 0x25, 0x00 };

// revision code
//
static unsigned int g_rev;

#include <stdlib.h>

#define BLKSIZE    0x4000
#define BUF_ADDR   0xff800000

void memory_dump(unsigned char *buf, unsigned long n)
{
	int i = 0;
	int c = 0;

	while (i < n) {
		printf("%02x ", buf[i]);
		c++;
		if (c == 16) { c = 0; printf("\n"); }
		i++;
	}
	if (c)
		printf("\n");
}

static
const char *
g_exceptiondesc[] = {
	"EMUEXCPT instruction issued",
	"EMUIN asserted (JTAG emulator exception)",
	"Watchpoint event occured",
	"",
	"Performance Monitor 0 overflow",
	"Performance Monitor 1 overflow",
	"", "",
	"Emulation single step"
};

void dump_dbgstat(CPU cpu)
{
	BFIN_REGISTER dbgstat;
	dbgstat = get_dbgstat(cpu);

	printf(g_line);
	if (dbgstat & DBGSTAT_CORE_FAULT)
		printf("!!! CORE FAULT !!!\n");
	
	printf("DBGSTAT: %08lx\n", dbgstat);

	if (dbgstat & DBGSTAT_IN_RESET)
		printf("In Reset\n");

	if (dbgstat & DBGSTAT_EMUACK)
		printf("Emulation Request Acknowledged\n");

	if (dbgstat & DBGSTAT_EMUREADY) 
		printf("Emulation Ready\n");

	if (dbgstat & DBGSTAT_EMUDOF)
		printf("Emudat Out Full!\n");

	if (dbgstat & DBGSTAT_EMUDIF)
		printf("Emudat In Full!\n");

	if (dbgstat & DBGSTAT_EMUDOOVF)
		printf("Emudat Out Overflow!\n");

	if (dbgstat & DBGSTAT_EMUDIOVF)
		printf("Emudat In Overflow!\n");

	if (dbgstat & DBGSTAT_BIST_DONE)
		printf("BIST not completed!\n");

	if (dbgstat & DBGSTAT_LPDEC0)
		printf("Loop 0 just decremented!\n");

	if (dbgstat & DBGSTAT_LPDEC1)
		printf("Loop 1 just decremented!\n");

	if (dbgstat & DBGSTAT_IDLE)
		printf("Core is IDLE!\n");

	if (dbgstat & DBGSTAT_EMUACK)
		printf("Emucause: %s\n", g_exceptiondesc[EMULATION_CAUSE(dbgstat)]);
	printf(g_line);
}

int mem_cmp(unsigned char *in, unsigned char *out, unsigned long size)
{
	int i;
	for (i = 0; i < size; i++) {
		if (in[i] != out[i]) {
			printf("Mismatch at %d\n", i);
			printf("In:\n");
			memory_dump(&in[i], 16);
			printf("Out:\n");
			memory_dump(&out[i], 16);
			return 1;
		}
	}
	return 0;
}

int init_EBIU(CPU cpu)
{
	set_memory_word(EBIU_SDGCTL, 0x0091998d, LDST_32);
	set_memory_word(EBIU_SDBCTL, 0x00000013, LDST_32);
	set_memory_word(EBIU_SDRRC,  0x00000817, LDST_32);
	return 0;
}


unsigned char g_buf[BLKSIZE];
unsigned char g_out[BLKSIZE];

int test_memory(CPU cpu)
{
	unsigned long l1addr = 0xffa00000;
	unsigned char *buf = g_buf;
	unsigned char *out = g_out;

	int error;

	TIMEVAL t0, t1;
	float t;

	int i;

	for (i = 0; i < BLKSIZE; i++) {
		buf[i] = rand();
	}

	int n = 0x4000;

	printf("Writing L1 data...\n");

	gettime(&t0);
	set_memory(cpu, 0xff800000, n, buf);
	gettime(&t1);
	t = took_time(&t0, &t1);
	printf("rate: %f kB/s\n", n / 1000.0 / t);
	
	printf("Reading L1 data...\n");
	gettime(&t0);
	get_memory(cpu, 0xff800000, n, out);
	gettime(&t1);
	t = took_time(&t0, &t1);
	printf("rate: %f kB/s\n", n / 1000.0 / t);

	if (mem_cmp(buf, out, n)) {
		printf("Memory test failed, aborting.\n");
		goto failure;
	}

	printf(g_line);
	printf("Writing SDRAM...\n");
	int size = BLKSIZE;
	int addr = 0x4000;
	gettime(&t0);
	set_memory(cpu, addr, size, buf);
	gettime(&t1);
	t = took_time(&t0, &t1);
	printf("rate: %f kB/s\n", size / 1000.0 / t);

	memset(out, addr, size);
	printf("Reading SDRAM...\n");
	gettime(&t0);
	get_memory(cpu, addr, size, out);
	gettime(&t1);
	t = took_time(&t0, &t1);
	printf("rate: %f kB/s\n", size / 1000.0 / t);

	printf("Comparing...\n");
	if (mem_cmp(buf, out, size)) {
		printf("Memory test failed, aborting.\n");
		goto failure;
	}
	printf("SDRAM memory good.\n");

	printf(g_line);
	printf("Test MDMA\n");
	if (size > 0x3ff0) size = 0x3ff0;

	printf("Write...\n");
	set_memory(cpu, BUF_ADDR, size, buf);
	printf("DMA -> L1 SRAM\n");
	error = init_dma(cpu, l1addr, BUF_ADDR, size);
	if (error < 0) goto failure;
	
	printf("DMA <- L1 SRAM\n");
	error = init_dma(cpu, BUF_ADDR, l1addr, size);
	if (error < 0) goto failure;

	printf("Read...\n");
	memset(out, 0, size);
	get_memory(cpu, BUF_ADDR, size, out);
	if (mem_cmp(buf, out, size)) {
		printf("Memory DMA transfer failed\n");
		goto failure;
	}
	printf("Memory DMA good.\n");

	return 1;
failure:
	emulation_leave(cpu);
	return 0;
}

int test_step(CPU cpu)
{
	int i;
	BFIN_REGISTER pc;
	emulation_enter_singlestep(cpu);

	printf("Testing single stepping\n");

	for (i = 0; i < 10; i++) {

		while (!(get_dbgstat(cpu) & DBGSTAT_EMUREADY)) {
			SLEEP(100);
		}
		pc = get_cpuregister(cpu, REG_RETE);
		printf("PC: %08lx\n", pc);
		// dump_registers(cpu);
		emulation_go();
	}

	emulation_leave_singlestep(cpu);
	return 1;
}

int test_breakpoints(CPU cpu)
{
	BFIN_REGISTER pc, dbgstat;
	unsigned char backup[2];
	int i;

	// now set breakpoint:
	
	int addr;
	
	if (g_rev == 2) {
		addr = 0x07018a00;
	} else {
		addr = 0x07f91398;
	}

	emulation_enter(cpu);
	get_memory(cpu, addr, 2, backup);
	printf("Old command: %02x %02x\n", backup[0], backup[1]);
	set_memory(cpu, addr, 2, g_emuexcpt);
	
	emulation_leave(cpu);

	while (!(get_dbgstat(cpu) & DBGSTAT_EMUREADY)) {
		printf("Waiting for exception...\n");
		SLEEP(200);
	}

	for (i = 0; i < 10; i++) {
		while (!((dbgstat = get_dbgstat(cpu)) & DBGSTAT_EMUREADY));
		printf("BREAKPOINT (%d)\n", i);
		if (dbgstat & DBGSTAT_CORE_FAULT) {
			printf("!!! Core fault !!!\n");
			return 0;
		}

		if (EMULATION_CAUSE(dbgstat) != EMUCAUSE_EMUEXCPT) {
			printf("Warning: emulation exception not caused by EMUEXCPT\n");
		}

		emulation_enter(cpu);

		// restore command
		pc = get_cpuregister(cpu, REG_RETE);
		pc -= 2;
		set_cpuregister(cpu, REG_RETE, pc);
		printf("    PC: %08lx\n", pc);
		printf("    R0: %ld\n", get_cpuregister(cpu, REG_R0));

		// Enter single step mode
		set_memory(cpu, addr, 2, backup);
		emulation_enter_singlestep(cpu);
		emulation_go();
		dbgstat = get_dbgstat(cpu);
		if (!(dbgstat & DBGSTAT_EMUREADY) ||
				((dbgstat >> 6) & 0xf) != EMUCAUSE_STEP) {
			printf("Could not single step!!\n");
			return 0;
		}
		// Set breakpoint again:
		set_memory(cpu, addr, 2, g_emuexcpt);

		emulation_leave_singlestep(cpu);
		// And go:
		emulation_leave(cpu);

		SLEEP(10);
	}

	// Restore code
	emulation_enter(cpu);
	set_memory(cpu, addr, 2, backup);
	emulation_leave(cpu);
	return 1;
}

#define DEF_REG(x) {REG_##x, #x}

static struct _regrepr {
	int regno;
	const char *repr;
} g_registers[] = {
	DEF_REG(RETE),
	DEF_REG(R0),
	DEF_REG(R1),
	DEF_REG(R2),
	DEF_REG(R3),
	DEF_REG(R4),
	DEF_REG(R5),
	DEF_REG(R6),
	DEF_REG(R7),
	DEF_REG(P0),
	DEF_REG(P1),
	DEF_REG(P2),
	DEF_REG(P3),
	DEF_REG(P4),
	DEF_REG(P5),
	DEF_REG(SP),
	DEF_REG(FP),
	DEF_REG(SEQSTAT),
	DEF_REG(SYSCFG),
	{ -1, "" }
};


void dump_registers(Cpu *cpu)
{
	struct _regrepr *r;
	BFIN_REGISTER val;

	r = g_registers;
	while (r->regno != -1) {
		val = get_cpuregister(cpu, r->regno);
		printf("%8s = %08lx\n", r->repr, val);
		r++;
	}
}

int test_register(CPU cpu)
{
	BFIN_REGISTER r0;

	r0 = get_cpuregister(cpu, REG_R0);
	set_cpuregister(cpu, REG_R0, 0xaa55aa55);
	if (get_cpuregister(cpu, REG_R0) != 0xaa55aa55) {
		printf("Error: R0 was not properly written\n");
		return 0;
	}
	set_cpuregister(cpu, REG_R0, r0);


	TIMEVAL t0, t1;
	gettime(&t0);
	dump_registers(cpu);
	gettime(&t1);
	float t = took_time(&t0, &t1);
	printf("took %f\n", t);
	return 1;
}

////////////////////////////////////////////////////////////////////////////
// Aux
int emu_ready(CPU cpu)
{
	BFIN_REGISTER r;
	r = get_dbgstat(cpu);
	if (r & DBGSTAT_EMUREADY) {
		return r;
	} else {
		return 0;
	}
}


void test_wpu(CPU cpu)
{
	// int addr = 0x7018786;
	int addr = 0x07f91396;
	BFIN_REGISTER pc, dbgstat;
	BFIN_REGISTER r;

	dbgstat = 0;  // PSW stop compiler warning

	emulation_enter(cpu);

	wpu_set_hbreak(cpu, 0, addr, 1);

	emulation_leave(cpu);

	do {
		printf("Waiting for exception...\n");
		SLEEP(200);
		dbgstat = get_dbgstat(cpu);
	} while (!(dbgstat & DBGSTAT_EMUREADY));

	if (EMULATION_CAUSE(dbgstat) != EMUCAUSE_WATCHPT) {
		printf("Warning: Emu exception was not caused by watch point\n");
		printf("Dbgstat: %08lx\n", dbgstat);
	}

	emulation_enter(cpu);
		dbgstat = get_dbgstat(cpu);

	// Restore PC
	pc = get_cpuregister(cpu, REG_RETE);
	printf("PC: %08lx\n", pc);

	printf("Hit HW breakpoint, disabling\n");
	wpu_set_hbreak(cpu, 0, addr, 0);

	get_memory_word(WPSTAT, &r, LDST_32);
	printf("WPSTAT: %08lx\n", r);

	// pc -= 2;
	// set_cpuregister(cpu, REG_RETE, pc);

	emulation_go();

}

void test_wpu_data(CPU cpu)
{
	int addr = 0x0200;
	BFIN_REGISTER pc, dbgstat;
	BFIN_REGISTER r;

	emulation_enter(cpu);

	wpu_init(cpu, 1);

	get_memory_word(WPIACTL, &r, LDST_32);
	printf("WPIACTL: %08lx\n", r);

	wpu_set_watch(cpu, 8, addr, WP_ALL);

	get_memory_word(WPDACTL, &r, LDST_32);
	printf("WPDACTL: %08lx\n", r);

	get_memory_word(WPDA0, &r, LDST_32);
	printf("WPDA0: %08lx\n", r);

	emulation_leave(cpu);

	while (!((dbgstat = get_dbgstat(cpu)) & DBGSTAT_EMUREADY)) {
		printf("Waiting for exception...\n");
		SLEEP(200);
	}

	if (EMULATION_CAUSE(dbgstat) != EMUCAUSE_WATCHPT) {
		printf("Warning: Emu exception was not caused by watch point\n");
		printf("Emu cause: %s\n", g_exceptiondesc[EMULATION_CAUSE(dbgstat)]);
	}

	emulation_enter(cpu);

	pc = get_cpuregister(cpu, REG_RETE);
	printf("PC: %08lx\n", pc);

	printf("Hit HW watchpoint, disabling\n");
	wpu_set_watch(cpu, 0, addr, 0);

	wpu_init(cpu, 0);

	emulation_leave(cpu);

}



////////////////////////////////////////////////////////////////////////////

void dump_dbgctl(Cpu *c)
{
	BFIN_REGISTER out;

	scanchain_select(SCAN_DBGCTL);
	jtag_goto_state(c->controller, s_jtag_shift_dr);
	scanchain_shiftout(&out, UPDATE);
	printf("DBGCTL: %08lx\n", out);
}


void mem_shortread(unsigned long addr, unsigned int n)
{
	int i;
	long r;

	printf("Word dump: \n");
	for (i = 0; i < n; i++) {
		get_memory_word(addr, &r, LDST_32);
		printf("%02x %02x ", (unsigned int) (r & 0xff), //PSW 
		                     (unsigned int)(r >> 8) & 0xff); //PSW
		printf("%02x %02x ", (unsigned int)((r >> 16) & 0xff), 
		                     (unsigned int)(r >> 24) & 0xff); //PSW
		addr += 4;
	}
	printf("\n");
}

	
extern char * const cable_params[];


int main(int argc, char **argv)
{
	int error = 0;
	CPU cpu;
	int ret;
        int testid=0;

        if (argc == 2 ) {
	    printf("Usage %s parallel addr type [testid]\n",argv[0]);
	    printf("%s parallel 0x378 WIGGLER\n",argv[0]);
	    return 0;
	}
        if (argc >= 4 ) {
	    printf("%s %s %s %s \n",argv[0],argv[1],argv[2],argv[3]);
            //return 0;
	    ret = jtag_init(&argv[1], &cpu);
	    if (argc >= 5 ) {
		if (strncmp(argv[4],"testid",6) == 0 ) {
		    testid = 1;
		}
	    }
	} else {
	    argv = (char **)cable_params;
	    ret = jtag_init(argv, &cpu);
	}
	if (ret < 0) {
		printf("Could not open jtag controller\n");
		return -1;
	}

	printf("Initializing emulation\n");
	emulation_init(cpu);

	if (testid) {
	    int jtag_getid(Cpu *cpu,unsigned long * id);
	    
	    int i;
	    unsigned long id;
	    
	    printf("Getting IDCODE many times..\n");
	    
	    for (i = 0; i < 1000000; i++) {
		if (( i % 10000) == 0 ) {
		    printf("get IDCODE ran %06d times\n",i);
		}
		jtag_getid(cpu, &id);
		if ((id & 0x0fffffff) != 0x027a50cb ) {
		    printf("Got wrong id after %d attempts: %08lx\n", 
			   i, id);
		}
	    }
	    jtag_exit(cpu);
	    return 0;
	}


	emulation_enter(cpu);
	if (!emu_ready(cpu)) {
		printf("Emulator not ready!\n");
		error = -1; goto failure;
	}

	char *s;
	printf("Detecting device...\n");
	s = detectDevice(cpu, &g_rev);
	if (s) {
		printf("Detected Device: %s rev:%d\n", s, g_rev);
	} else {
		printf("Could not detect device, JTAG failure ?\n");
		goto failure;
	}

	printf(g_dline);

	printf("Resetting...\n");
	cpu_reset(cpu, 3);
	printf("Init EBIU...\n");
	init_EBIU(cpu);


	// test register access
	printf(g_dline);
	printf("Register test\n");
	if (test_register(cpu) == 0) {
		error = -1; goto failure;
	}
	printf(g_dline);
	
	SLEEP(500);

	unsigned long v;
	printf("Testing MMR access...\n");
	get_memory_word(DSPID, &v, LDST_32);
	printf("DSPID: %08lx\n", v);

#if 0
	// test single word read
	printf(g_dline);
	printf("Single word read:");
	TIMEVAL t0, t1;
	
	gettime(&t0);
	mem_shortread(0xffa00000, 20);
	printf("\n");
	gettime(&t1);
	float t = took_time(&t0, &t1);
	printf("rate: %f words/s\n", 20 / t);
#endif

	{
		int i;
		TIMEVAL t0, t1;
		unsigned long r;
		r = 0xffaa0055;
		gettime(&t0);
		for (i = 0; i < 20; i ++) {
			set_memory_word(0, r, LDST_32);
		}
		gettime(&t1);
		float t = took_time(&t0, &t1);
		printf("rate: %f words/s\n", 20 / t);

		get_memory_word(0, &r, LDST_32);
		printf("word: %08lx\n", r);

	}

	// test memory access
	printf("Memory test\n");
	{
		int i; 
		for (i = 1; i <= 1; i++) {
			printf(g_dline);
			printf("// Run %02d //\n", i);

			if (!test_memory(cpu)) {
				error = -1; goto failure;
			}
		}
	}

	// test single stepping
	printf(g_dline);
	test_step(cpu);
	printf(g_dline);
	emulation_leave(cpu);

	SLEEP(200);

	if (((cpu->id & 0x0fffffff) == IDCODE_BF533 && g_rev >= 3 ) || 
	    ((cpu->id & 0x0fffffff) == IDCODE_BF537 ) ) {
		test_wpu_data(cpu);
	} else {
		printf("NO watchpoints on this silicon revision - skip test\n");
	}

	// test breakpoints
	printf(g_dline);
	if (test_breakpoints(cpu) == 0) {
		error = -1; goto failure;
	}

	printf("done, leaving emulation.\n");

failure:

	emulation_exit(cpu);
	jtag_exit(cpu);

	return error;
}
